//
// Created by s on 2025/10/14.
//

#include "interface.h"


#include <string.h>
__IO uint8_t rx_data[8];
__IO uint8_t cmd[13];
__IO bool isRxTarget = false;

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart->Instance == huart3.Instance)
    {
        HAL_UART_Receive_DMA(&huart3, rx_data, sizeof(rx_data));

    }
}


void UART_Send(uint8_t *data, uint8_t length)
{
    HAL_UART_Transmit_DMA(&huart3, data, length);
    HAL_Delay(10);
}

/*
 * 速度控制
 * 输入：id, direction, velocity, acceleration, all_flag
 */
void VelControl(uint8_t id, uint8_t direction, uint16_t speed, uint8_t acceleration, bool all_flag)
{
    cmd[0] = id;
    cmd[1] = 0xF6;
    cmd[2] = direction;

    /*速度处理*/
    cmd[4] = *(uint8_t *) &speed;
    cmd[3] = *((uint8_t *) &speed + 1);

    cmd[5] = acceleration;
    cmd[6] = all_flag;
    cmd[7] = 0x6B;

    UART_Send(cmd, 8);
}

/*
 * 位置控制
 * 输入：id, direction, velocity, angle,re_ab_flag, acceleration, all_flag
 */
void PosControl(uint8_t id, uint8_t direction, uint16_t speed, uint8_t acceleration, float degree, bool re_ab_flag, bool all_flag)
{
    cmd[0] = id;
    cmd[1] = 0xFD;
    cmd[2] = direction;

    /*速度处理*/
    cmd[4] = *(uint8_t *) &speed;
    cmd[3] = *((uint8_t *) &speed + 1);

    cmd[5] = acceleration;

    /*角度处理, 从角度转换为脉冲数*/
    uint32_t pauls = (uint32_t)roundf((80.0f / 9.0f) * degree);
    cmd[9] = *(uint8_t *) (&pauls);
    cmd[8] = *((uint8_t *) (&pauls) + 1);
    cmd[7] = *((uint8_t *) (&pauls) + 2);
    cmd[6] = *((uint8_t *) (&pauls) + 3);

    cmd[10] = re_ab_flag;
    cmd[11] = all_flag;
    cmd[12] = 0x6B;

    UART_Send(cmd, 13);
}

/*
 * 触发同步运动
 */
void MoveTrigger()
{
    cmd[0] = 0x00;
    cmd[1] = 0xFF;
    cmd[2] = 0x66;
    cmd[3] = 0x6B;
    UART_Send(cmd, 4);
}

/*
 * 电机急停
 * 输入： id, all_flag
 */
void EmergencyStop(uint8_t id, bool all_flag)
{
    cmd[0] = id;
    cmd[1] = 0xFE;
    cmd[2] = 0x98;
    cmd[3] = all_flag;
    cmd[4] = 0x6B;

    UART_Send(cmd, 5);
}

/*
 * 实时角度读取
 * 输入：id
 * 输出：角度(触发中断)
 */
void ReadAngle(uint8_t id)
{
    cmd[0] = id;
    cmd[1] = 0x36;
    cmd[2] = 0x6B;

    UART_Send(cmd, 3);
}

/*
 * 角度解算函数
 */
float ParserAngle(const uint8_t *receive_data)
{
    /*确定方向*/
    const float direction = receive_data[2] > 0x00 ? -1.0f : 1.0f;

    /*拼接数据*/
    uint32_t data_angle = __builtin_bswap32(*(uint32_t *) &receive_data[3]);

    /*角度转换*/
    const float current_angle = (direction * data_angle * 360.0f) / 65536.0f;

    return current_angle;
}
